﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Steering.SteeringAbstraction;

using Zion.Input;

namespace GloveAdapter
{
    public class GloveSteering : ForkliftSteeringSource
    {        
        private const int FINGER_BENT_VALUE = 20;
        private const int FINGER_STRAIGHT_VALUE = 10;
        private const int MOVEMENT_POINTS = 3;

        public override event EventHandler StopIssued = null;
        public override event EventHandler MenuIssued = null;

        private List<float> moveValues = new List<float>();
        private List<float> turnValues = new List<float>();
        private List<float> forkValues = new List<float>();

        private P5Dll gloveControl;

        public GloveSteering()
        {
            gloveControl = new P5Dll();
            Speed.Scale = 0.004;
            Speed.ZeroRange = 0.05;
            Turn.Scale = 0.004;
            Turn.ZeroRange = 0.05;
            ForkSpeed.Scale = 0.004;
            ForkSpeed.ZeroRange = 0.8;
        }

        public override bool StartSteering()
        {
            return gloveControl.Connect();
        }

        public override void StopSteering()
        {
            if (gloveControl != null && gloveControl.Connected)
                gloveControl.Close();
        }

        public override void UpdateSteering()
        {
            P5State state = gloveControl.GetState();

            moveValues.Add(state.RelativeZ);
            turnValues.Add(state.RelativeX);
            forkValues.Add(state.RelativeY);

            // if lists have more than the specified number of elements we delete the oldest ones
            if (moveValues.Count > MOVEMENT_POINTS)
            {
                moveValues.RemoveAt(0);
                turnValues.RemoveAt(0);
                forkValues.RemoveAt(0);
            }

            if (CheckSteering(state))
            {
                if(CheckOneDirection(moveValues))
                    this.Speed.ChangeValueSnapToZero(moveValues[moveValues.Count - 1] - moveValues[moveValues.Count - 2]);
                if(CheckOneDirection(turnValues))
                    this.Turn.ChangeValueSnapToZero(turnValues[turnValues.Count - 1] - turnValues[turnValues.Count - 2]);                
            }

            if (CheckFork(state) && CheckOneDirection(forkValues))
            {
                this.ForkSpeed.ChangeValueSnapToZero(forkValues[forkValues.Count - 1] - forkValues[forkValues.Count - 2]);

                // so that the fork spee will only equal min, max or 0
                if (this.ForkSpeed.Value > this.ForkSpeed.ZeroRange)
                    this.ForkSpeed.SetAbsoluteValue(this.ForkSpeed.Max);
                else if (this.ForkSpeed.Value < this.ForkSpeed.ZeroRange * (-1))
                    this.ForkSpeed.SetAbsoluteValue(this.ForkSpeed.Min);
            }

            if (CheckMenu(state))
                MenuIssued(this, null);
            else if (CheckStop(state))
                StopIssued(this, null);
        }

        private bool CheckSteering(P5State state)
        {
            if (state.Finger[1] > FINGER_BENT_VALUE &&
                state.Finger[2] > FINGER_BENT_VALUE &&
                state.Finger[3] > FINGER_BENT_VALUE &&
                state.Finger[4] > FINGER_BENT_VALUE)
                return true;
            else
                return false;
        }

        private bool CheckFork(P5State state)
        {
            if (state.Finger[1] < FINGER_STRAIGHT_VALUE &&
                state.Finger[2] > FINGER_BENT_VALUE &&
                state.Finger[3] > FINGER_BENT_VALUE &&
                state.Finger[4] > FINGER_BENT_VALUE)
                return true;
            else
                return false;
        }

        private bool CheckStop(P5State state)
        {
            if (state.Finger[1] < FINGER_STRAIGHT_VALUE &&
                state.Finger[2] < FINGER_STRAIGHT_VALUE &&
                state.Finger[3] > FINGER_BENT_VALUE &&
                state.Finger[4] > FINGER_BENT_VALUE)
                return true;
            else
                return false;
        }

        private bool CheckMenu(P5State state)
        {
            if (state.Finger[1] > FINGER_BENT_VALUE &&
                state.Finger[2] < FINGER_STRAIGHT_VALUE &&
                state.Finger[3] < FINGER_STRAIGHT_VALUE &&
                state.Finger[4] < FINGER_STRAIGHT_VALUE)
                return true;
            else
                return false;
        }

        /// <summary>
        /// Checks if all elements of the list are greater than the first one
        /// or if they are all lesser than it.
        /// This method should prevent jittering of the glove indications.
        /// </summary>
        /// <param name="values">The list.</param>
        /// <returns>True if all elements are greater or all are smaller than the first element.
        /// Returns false if there is less then 3 elements on the list.</returns>
        private bool CheckOneDirection(List<float> values)
        {
            if (values.Count < 3)
                return false;

            float firstValue = values[0];
            bool valuesGreater = values[1] > firstValue;

            for (int i = 2; i < values.Count; i++)
            {
                // next values should be greater
                if (valuesGreater && firstValue > values[i])
                    return false;   // but one was smaller
                if (!valuesGreater && firstValue < values[i])
                    return false;
            }
            return true;
        }

        // use this eventually to minimise jittering
        private float GetMeanChange(List<float> values)
        {
            float sumChange = 0;
            if (values.Count < 2)
                return 0;

            for (int i = 1; i < values.Count; i++)
                sumChange += values[i] - values[i - 1];

            /* There is values.Count values, so there is values.Count-1 differences.
             * We get mean difference*/
            return sumChange / (values.Count - 1);
        }
    }
}
